#ifndef SD_LINALG_HPP
#define SD_LINALG_HPP

#include "config.hpp"
#include "discrete.hpp"
#include <algorithm>

namespace MT {

//! polar-decompose square matrix A into P and U such that
//!   A = U P
//! U = unitary matrix
//! P = positive-semidefinite Hermitian matrix
template<typename _Scalar, int _Rows>
void polar_decompose(Eigen::Matrix<_Scalar, _Rows, _Rows> const &A,
    Eigen::Matrix<_Scalar, _Rows, _Rows> &U,
    Eigen::Matrix<_Scalar, _Rows, _Rows> &P)
{
    typedef Eigen::SVD<Eigen::Matrix<_Scalar, _Rows, _Rows> > SVD;
    SVD svd(A); // A = U D V'
    U = svd.matrixU()*(svd.matrixV().transpose()); // OutputU = U V'
    P = svd.matrixV()*(svd.singularValues().asDiagonal())*(svd.matrixV().transpose()); // P = V D V'
}


//! compute PCA
//! Input:
//!   A : DxN matrix consisting of N D-dimensional points
//! Output:
//!   R : DxD unit orthogonal matrix consisting of principal vectors corresponding to D
//!   D : diagonal vector of eigenvalues sorted in an descending order
template<typename _Scalar, int _Rows>
void pca(Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic> const &A,
    Eigen::Matrix<_Scalar, _Rows, _Rows> &R, Eigen::Matrix<_Scalar, _Rows, 1> &D)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<_Scalar, _Rows, _Rows> > solver(A*A.transpose());
    int N = A.rows(), i;
    R.resize(N, N);
    D.resize(N, 1);
    Eigen::Matrix<int, _Rows, 1> id = argsort<_Scalar, _Rows>(solver.eigenvalues(), false);
    for(i = 0; i < N; ++i)
    {
        R.col(i) = solver.eigenvectors().col(id[i]);
        D[i] = solver.eigenvalues().coeff(id[i]);
    }
}


//! compute the integral image
//!   J(row, col) = sum_{row' <= row, col', <= col} I(row', col')
//! Remark: in-place computation is supported, result can be mat
template<typename _Scalar, int _Rows, int _Cols>
void int2d(Eigen::Matrix<_Scalar, _Rows, _Cols> const &mat, Eigen::Matrix<_Scalar, _Rows, _Cols> &result)
{
    _Scalar *ddst;
    _Scalar const *dsrc;
    int src_step, dst_step, rows, cols;

    rows = mat.rows(); cols = mat.cols(); dsrc = mat.data();
    result.resize(rows, cols);
    ddst = result.data();
    if(cols > 1) { src_step = &mat(0,1) - dsrc; dst_step = &result(0,1) - ddst; }

    int i, j;

    ddst[0] = dsrc[0];
    if(rows > 0) for(i = 1; i < rows; ++i) ddst[i] = ddst[i-1]+dsrc[i];
    if(cols > 0) for(j = 1; j < cols; ++j)
        ddst[(j*dst_step)] = ddst[(j*dst_step)-dst_step]+dsrc[j*src_step];
    if(rows > 1 && cols > 1) for(j = 1; j < cols; ++j)
    {
        _Scalar const *ds = &dsrc[j*src_step];
        _Scalar *dd = &ddst[j*dst_step];
        for(i = 1; i < rows; ++i) dd[i] = (dd[(i-dst_step)]-dd[(i-dst_step)-1])+dd[i-1]+ds[i];
    }
}


} // namespace MT

#endif
